
#ifndef __MLS_MESHDEFORMER_H__
#define __MLS_MESHDEFORMER_H__

#include "../core/mlsDeformer.h"
#include "mesh.h"

//~ extern vector<Point3> pstarl;
//~ extern vector<Point3> qstarl;

template <class T>
class MlsMeshDeformer : public MlsDeformer<T> {
   
   typedef TConstraints<Point3>     PConstraints;
   typedef TConstraints<Segment3>   SConstraints;
public:
   ~MlsMeshDeformer() {}
   
   MlsMeshDeformer()
      :MlsDeformer<T>::MlsDeformer(), model(0), isGeodesic(true) {
      this->coSource = 0;
      this->coTarget = 0;
   }
   
   /// 
   /// @param m the mesh model
   /// @param constraints the initial constraints
   MlsMeshDeformer(Mesh *m, const T &constraints, bool flag=true)
      :MlsDeformer<T>::MlsDeformer(), isGeodesic(flag) {
      
      // copy the initial positions
      assert(m != 0);
      model = m;
      ipositions.resize(model->nvertices());
      inormals.resize(model->nvertices());
      Vertex_iterator vi = model->vertices_begin();
      for (unsigned int i=0; i<ipositions.size(); ++i, ++vi) {
         ipositions[i] = vi->point();
         inormals[i] = vi->normal();
      }
      
      cosourcE = constraints;
      this->coSource = &constraints;
      
      // generate the table of distances
      genDistanceTable(this->coSource);
      // generate the table of weights
      genWeightTable(this->coSource);
   }
   
   /// Generates the table of weights. Each entry [i][j] maps the weight 
   /// corresponding between the joint i and the constraint j
   virtual void genWeightTable(const PConstraints *constraints) {
      
      assert(constraints != 0);
      assert(not this->distanceTable.empty());
      
      cout<<" ... computing the weights table .... "<<endl;
      this->weightTable.resize(this->distanceTable.size());
      for (uint i=0; i<this->weightTable.size(); ++i) {
         const uint &n = this->distanceTable[i].size();
         this->weightTable[i].resize(n, 0.0);
         for (uint j=0; j<n; ++j) {
            this->weightTable[i][j] = this->omega(this->distance(i, j));
         }
      }
      cout<<" ... computing the weights table .... done "<<endl;
   }
   
   virtual void genDistanceTable(const PConstraints *constraints) {
      
      cout<<" ... computing the distance table mlsmeshdeformer... "<<endl;
      this->distanceTable.resize(this->model->nvertices());
      for (uint i=0; i<this->distanceTable.size(); ++i) {
         this->distanceTable[i].resize(constraints->size(),0.0);
      }
      
      bool isPossible = false; /// its possible to compute a geodesic metric over the structure?
      unsigned int nminusone = std::count(constraints->ids.begin(), constraints->ids.end(), -1);
      // If all constraints are associated with vertices then it is possible
      if (nminusone == 0) isPossible = true;
      
      if (isPossible and isGeodesic) {
         cout<<" ... computing vertex/constraints(vertex) distances walking over the mesh surface ... "<<endl;
         /// fill the distance table. Each entry has the value [vertice][constraint]
         for (uint j=0; j<constraints->size(); ++j) {
            model->floodfill_Dijkstra(constraints->ids[j]);
            Vertex_iterator vit = model->vertices_begin();
            for (uint i=0; i<model->nvertices(); ++i, ++vit) {
               this->distanceTable[i][j] = vit->d;
            }
         }
      } else {
         cout<<" ... computing euclidian distances ... "<<endl;
         for (uint i=0; i<model->nvertices(); ++i) {
            for (uint j=0; j<constraints->size(); ++j) {
               const Point3 &P = ipositions[i];
               const Point3 &p = constraints->getConstraint(j);
               Vector3 v(p,P);
               this->distanceTable[i][j] = sqrt(v*v);
            }
         }
      }
   }
   
   virtual void genWeightTable(const SConstraints *constraints) {
      
      assert(this->model != 0);
      assert(constraints != 0);
      
      // generate the table of distances
      //genDistanceTable(constraints);
      
      cout<<" ... computing the weights table ... begin"<<endl;
      const uint nvertices = this->model->nvertices();
      const uint nconstraints = constraints->size();
      
      this->weightMatrixTable.resize(nvertices);
      for (uint i=0; i<nvertices; ++i) {
         this->weightMatrixTable[i].resize(nconstraints, Matrix(2,2));
         for (uint j=0; j<nconstraints; ++j) {
            Matrix &W = this->weightMatrixTable[i][j]; /// matrix of weights
            Number d, t;
            this->distance(i,j,d,t); // return the distance to the bone j. d:distance, t:parameter for define the closest point
            const Number peso = this->omega(d);
            
            /// constant version
            //~ W(0,0)=peso/3.0;  W(0,1)=peso/6.0;
            //~ W(1,0)=W(0,1);    W(1,1)=W(0,0);
            
            /// parametrized version
            W(0,0)=peso*t*t;       W(0,1)=peso*t*(1.0-t);
            W(1,0)=W(0,1);         W(1,1)=peso*(1.0-t)*(1.0-t);
            
            //~ W(0,0)=peso/6.0;  W(0,1)=peso/3.0;
            //~ W(1,0)=W(0,1);    W(1,1)=W(0,0);
         }
      }
      cout<<" ... computing the weights table ... done"<<endl;
   }
   
   virtual void genDistanceTable(const SConstraints *constraints) {
      
      cout<<" ... computing the distance table (dt: for segments) ... "<<endl;
      this->dtTable.resize(this->model->nvertices());
      for (uint i=0; i<this->dtTable.size(); ++i) {
         this->dtTable[i].resize(constraints->size(),make_pair(0.0,0.0));
      }
      
      bool isPossible = false; /// no tiene sentido tener aristas constraints en la malla
      
      if (isPossible and isGeodesic) {
         cout<<" ... computing geodesic distances vertex/constraints(segment) ... "<<endl;
      } else {
         cout<<" ... computing euclidian distances vertex/constraints(segment) ... "<<endl;
         Number t;
         for (uint i=0; i<model->nvertices(); ++i) {
            const Point3 &P = ipositions[i];
            for (uint j=0; j<constraints->size(); ++j) {
               const Segment3 &segment = constraints->getConstraint(j);
               Number d = computeDistanceMinSegment(P,segment,t);
               this->dtTable[i][j] = make_pair(d,t);
            }
         }
      }
   }
   
   virtual void execute(const T &constraints) {
      
      this->coTarget = &constraints; // the target constraints
      int i=0;
      Vertex_iterator vi = model->vertices_begin();
      while(vi != model->vertices_end()) {
         vi->point() = eval(i);
         vi->normal() = this->rotate(inormals[i]);
         ++vi; ++i;
      }
   }
   
   Point3 eval(int i) {
      
      const Point3 &p = ipositions[i];
      this->pstar = computeCentroid(i, &this->cosourcE);
      this->qstar = computeCentroid(i, this->coTarget);
      
      //~ pstarl[i] = this->pstar;
      //~ qstarl[i] = this->qstar;
      
      Vector3 v(this->pstar, this->qstar), rv; 
      //~ if (sqrt(v*v) > 0.001) {
         //~ cout<<"p*,q*: "<<p<<" : "<<this->pstar<<" : "<<this->qstar<<endl;
         computeCorrelationMatrix(i, &this->cosourcE, this->coTarget);
         Number maxroot = this->computeLargestRootOfQuarticEquation();
         this->eigenvalue = maxroot + this->E;
         this->computeRotationVector(); // computes the u value
         //~ this->u[0] = 0; this->u[1] = 0; this->u[2] = -1;
         rv = this->rotate(p-this->pstar);
         //~ cout<<"u: "<<this->u[0]<<" "<<this->u[1]<<" "<<this->u[2]<<endl;
      //~ }
      //~ else 
         //~ rv = p-this->pstar;
      return Point3(this->qstar[0]+rv[0], this->qstar[1]+rv[1], this->qstar[2]+rv[2]);
   }
   
protected:
   Mesh                      *model;
   vector<Point3>             ipositions; // initial vertex positions
   vector<Vector3>            inormals; // initial vertex normal
   bool                       isGeodesic; // true if the geodesic metric will be used
   
   T                          cosourcE;
};

#endif
